
#include "global_test.h"

using namespace cv;
using namespace std;

Mat target_stack;
vector<Point2f> target_stack_vect;

double angular_vel = 0;
double linear_vel = 0;

//ArRobot robot;




int main(int argc, char* argv[])
{
	/*
	// Initialization
    float	target_pos, obstacles_pos, potential; // values for input variables
    char	option;	// var for selection of what user wants to do
	int ret_val;

	double steering_param, velocity_param;
	
	char c;

	Mat image;
	char* file = "expmap.jpg";

	//target_stack = Mat::zeros() -> use a  dynamic vector



	// get obstacle positions
	int* out;
	int m, n;
	myGetObstacleCoord(&out, m, n);

	Mat obstacles = Mat::zeros(1, m*n/2, CV_32FC2);

	for (int i=0; i<m*n; i++)
	{
		if(i%2==0)
		{
			printf("%5d",out[i]);
			obstacles.at<Vec2f>(0,i/2)[0] = out[i];
		}
		else
		{
			printf(" %5d ",out[i]);
			obstacles.at<Vec2f>(0,i/2)[1] = out[i];
		}
	}

	TargetFeeder tf = TargetFeeder(image, obstacles);

	char* wind_name = "Choose Workspace";
	char c;

	// draw obstacles on image
	drawCircles(tf.map_image, tf.map_obstacles);



	imshow(wind_name, tf.map_image);

	setMouseCallback(wind_name, wrappedOnMouse, (void*)&tf);

	//setMouseCallback(wind_name, on_mouse, 0);   // bug is here! cannot use mouseCallback

	// click 2 corners of workspace

	while (tf.clicked_points.size() < 2)
	{
		printf("Please click 2 POINTS to create workspace!!\n");

		c = cvWaitKey(1);
		if (c == 27) 
		{
			break;
		}
	}

	//	};

	// call generate
	tf.generate(10);

	// visualize trajectory
	tf.visualize();
	*/

	/*
	FuzzyController FzCtrl(ret_val); 

	if (ret_val < 0)
	{
		cout << "Error Opening fuzzycontroller.fcl";
		// make sure the "working directory" in "Project | Settings..."
		// is set to the executable's directory if running from the MSVC IDE
	}


	while (1)
	{ 
		// delay delta_t 

		// get obstacle status
		obstacles_pos = getObstaclePosition();

		// get target status
		target_pos = getTargetPosition();

		// get mode: virtual target OR real target
		
		
		// feed to fuzzy controller to have output decision 
		FuzzyController.getDecision(target_pos, obstacles_pos, potential);

		// execute decision
		// define K value
		steering_param = 1;
		velocity_param = 2;

		
		// call steering
		steering(steering_param, velocity_param);


		// press "Esc" to escape
		c = (char)waitKey(1);
		if (c == 27)
		{
			break;
		}
	}  
	*/
    return 0;

} // end main()


